/*
 * 本模块用于对编码器数据进行解析。
 * 编码器采用modbus协议通信，并将回报率上升至100Hz。
 * 使用方法首先在setup里INIT_AOAI()，通过SC16IS752发送100Hz的回报率。
 * 然后在available之后，调用AOAI_RX(XX.read())，解析数据。
 * 最后是检测AOAI_UPDATED()，函数会返回是否接收到一帧完整数据。
 * 想要获取攻角数据，调用AOAI_AOA()即可，将角度-30°到30°转换为-4096到4096
 */
#include "AOAI.h"
#include "IMU.h"
#include "hardwareDef.h"

uint8_t s_AOA[10] = {};
uint8_t i_AOA = 0;

int32_t aoa;
bool aoaNew = 0;

void INIT_AOAI(void)
{
  //00 06 00 D7 00 0A B8 24，编码器启动100Hz主动传输
  IMU_TX(SC16IS752_CHANNEL_A, 0x00);
  IMU_TX(SC16IS752_CHANNEL_A, 0x06);
  IMU_TX(SC16IS752_CHANNEL_A, 0x00);
  IMU_TX(SC16IS752_CHANNEL_A, 0xD7);
  IMU_TX(SC16IS752_CHANNEL_A, 0x00);
  IMU_TX(SC16IS752_CHANNEL_A, 0x0A);
  IMU_TX(SC16IS752_CHANNEL_A, 0xB8);
  IMU_TX(SC16IS752_CHANNEL_A, 0x24);
}

void AOAI_RX(uint8_t in)
{
  switch (i_AOA)
  {
  case 0://检测帧头
    if (in == 0x00)
    {
      for (uint8_t j = 0; j < 10; j ++)
      {
        s_AOA[j] = 0;
      }
      CRC(in, 1, 0);
      s_AOA[i_AOA] = in;
      i_AOA ++;
    }
    break;

  case 1:
    if (in == 0x03)
    {
      CRC(in, 0, 0);
      s_AOA[i_AOA] = in;
      i_AOA ++;
    }
    else
    {
      i_AOA = 0;
    }
    break;

  case 2:
     if (in == 0x04)
    {
      CRC(in, 0, 0);
      s_AOA[i_AOA] = in;
      i_AOA ++;
    }
    else
    {
      i_AOA = 0;
    }
    break;

  default:
    CRC(in, 0, 0);
    s_AOA[i_AOA] = in;
    i_AOA ++;
    break;

  case 7:
    if ((CRC(in, 0, 1) & 0xFF) == in)
    {
      i_AOA ++;
    }
    else
    {
      i_AOA = 0;
    }
    break;

  case 8:
    if ((CRC(in, 0, 1) >> 8) == in)
    {
      aoa = ((uint32_t)s_AOA[4] << 16) | ((uint32_t)s_AOA[5] << 8) | ((uint32_t)s_AOA[6]);
    }
    aoaNew = 1;
    i_AOA = 0;
    break;
  }
}

uint16_t CRC(uint8_t in, bool rset, bool out)
{
  static uint16_t crc;
  if (rset)
  {
     crc = 0xFFFF;
  }
  if (out) return crc;
  crc = crc ^ in;
  for(uint8_t j = 0; j < 8; j++)
  {
    if (crc & 0b1)
    {
      crc = crc >> 1;
      crc = crc ^ 0xA001;
    }
    else
    {
      crc = crc >> 1;
    }
  }
  return crc;
}

bool AOAI_UPDATED(void)
{
  bool old = aoaNew;
  aoaNew = 0;
  return old;
}

int32_t AOAI_AOA(void)
{
  if ((aoa - aoaZero) > 131072)
  {
    return (aoa - aoaZero - 262144) / 6;//  45/8/aoamax aoamax=33.75
  }
  else if ((aoa - aoaZero) < -131072)
  {
    return (aoa - aoaZero + 262144) / 6;//0.16875
  }
  else
  {
    return (aoa - aoaZero) / 6;
  }
}
